package com.hitqz.robot.driver.schedule;

import cn.hutool.core.bean.BeanUtil;
import cn.hutool.core.net.NetUtil;
import cn.hutool.core.util.StrUtil;
import cn.hutool.extra.spring.SpringUtil;
import com.google.common.base.Objects;
import com.google.common.util.concurrent.RateLimiter;
import com.hitqz.robot.api.business.enums.VoiceEnum;
import com.hitqz.robot.api.business.event.KcStopTriggerEvent;
import com.hitqz.robot.api.business.event.PlayVoiceEvent;
import com.hitqz.robot.api.common.dto.robot.RobotOnlineEventDto;
import com.hitqz.robot.api.common.dto.tickets.CtrlTicketsEvent;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.driver.communication.NetworkServiceThreadPool;
import com.hitqz.robot.driver.dto.KcLocationDto;
import com.hitqz.robot.driver.dto.KcRobotRunningStatus;
import com.hitqz.robot.driver.dto.KcStopStatusDto;
import com.hitqz.robot.driver.util.*;
import lombok.extern.slf4j.Slf4j;
import org.springframework.scheduling.annotation.Scheduled;
import org.springframework.stereotype.Component;

import java.text.DecimalFormat;
import java.util.Optional;
import java.util.concurrent.atomic.AtomicBoolean;

/**
 * 定时查询
 *
 * @author wengyx
 * @date 2024/09/02
 */
@Component
@Slf4j
public class QuerySchedule {

    private static boolean lastSpeedCtrl = false;

    private static final DecimalFormat df = new DecimalFormat("0.00");

    private final static RateLimiter publishHardStopStatusVoiceLimiter = RateLimiter.create(1);
    private final static RateLimiter publishFrontStopStatusVoiceLimiter = RateLimiter.create(1);
    private final static RateLimiter publishBackStopStatusVoiceLimiter = RateLimiter.create(1);

    /**
     * 定时轮询机器人运行状态
     *
     * @throws InterruptedException
     */
    @Scheduled(fixedDelay = 200)
    private void queryRunningStatus() throws InterruptedException {
        if (this.checkNet()) {
            NetworkServiceThreadPool.dealMessage(KcToolkit::queryRunningStatus);
        }
    }

    /**
     * 定时轮询机器人导航状态
     *
     * @throws InterruptedException
     */
    @Scheduled(fixedDelay = 500)
    private void queryNavStatus() throws InterruptedException {
        if (this.checkNet()) {
            NetworkServiceThreadPool.dealMessage(KcToolkit::queryNavStatus);
        }
    }


    /**
     * 轮询变量值
     */
    @Scheduled(fixedDelay = 500)
    private void queryVar() {
        if (this.checkNet()) {
            if (!DataTypeEnum.UN_QUERY_VAR_NAME_SET.isEmpty()) {
                log.info("[kc]批量移除不存在的变量");
                DataTypeEnum.UN_QUERY_VAR_NAME_SET.forEach(name -> {
                    DataTypeEnum.VAR_NAME_SET.remove(name);
                    DataTypeEnum.VAR_NAME_ENUM_MAP.remove(name);
                    log.info("[kc]移除变量:{}", name);
                });
                DataTypeEnum.UN_QUERY_VAR_NAME_SET.clear();
            }
            DataTypeEnum.VAR_NAME_SET.forEach(name -> {
                NetworkServiceThreadPool.dealMessage(() -> {
                    KcToolkit.readVar(name);
                });
            });
        }
    }


    /**
     * 控制速度时要定时100ms发心跳，1s内没发的话，控制器的组态软件会强制速度置0
     *
     * @throws InterruptedException
     */
    @Scheduled(fixedDelay = 100)
    private void sendCmdSpeedHeartbeat() throws InterruptedException {
        NetworkServiceThreadPool.dealMessage(() -> {
            if (this.checkNet() && KcCacheUtil.getSpeedFlag()) {
                KcToolkit.writeVar("cmd_heartbeat", DataTypeEnum.UDINT, String.valueOf(SequenceUtil.nextSpeedCmdHeartBeatId()));
            }
        });
    }

    /**
     * 切换控制模式-用于web遥控速度
     *
     * @throws InterruptedException
     */
    @Scheduled(fixedDelay = 250)
    private void changeCtrlMode() throws InterruptedException {
        boolean currentSpeedFlag = KcCacheUtil.getSpeedFlag();
        if (!Objects.equal(lastSpeedCtrl, currentSpeedFlag)) {
            String currentMode = KcToolkit.readVar("cmd_forcedctrl");
            if (currentSpeedFlag) {
                // 切换上位机模式 写速度变量才能生效
                boolean result = KcToolkit.writeVar("cmd_forcedctrl", DataTypeEnum.UDINT, "1");
                log.info("[kc] 切换上位机模式{}", result);
            } else {
                // 速度置 0
                KcToolkit.writeVar("cmd_vel_line_x", DataTypeEnum.REAL, "0.0");
                KcToolkit.writeVar("cmd_vel_line_y", DataTypeEnum.REAL, "0.0");
                KcToolkit.writeVar("cmd_vel_angle", DataTypeEnum.REAL, "0.0");
                // 切换上位机模式
                boolean result = KcToolkit.writeVar("cmd_forcedctrl", DataTypeEnum.UDINT, "0");
                log.info("[kc] 切换自动模式 {}", result);
            }
            String changedMode = KcToolkit.readVar("cmd_forcedctrl");
            log.info("[kc] cmd_forcedctrl {} -> {}", currentMode, changedMode);
        }
        lastSpeedCtrl = currentSpeedFlag;
    }


    /**
     * 减少查询频率
     *
     * @return
     */
    private boolean checkNet() {
        if (KcToolkit.client == null) {
            return false;
        }
        if (!NetUtil.ping(KcToolkit.kcControlIp, 2000)) {
            return false;
        }
        return KcToolkit.client.readyToSend();
    }


    @Scheduled(fixedDelay = 1000)
    public void checkLocation() {
        NetworkServiceThreadPool.dealEvent(() -> {
            if (KcToolkit.posMap == null || KcToolkit.posMap.isEmpty()) {
                return;
            }
            KcRobotRunningStatus runningStatus = (KcRobotRunningStatus) KcCacheUtil.get(KcCacheUtil.KEY_RUNNING_STATUS);
            if (runningStatus == null) {
                return;
            }
            //计算最近点
            double minDistance = Double.MAX_VALUE;
            RobotMapPos nearestPos = null;
            for (RobotMapPos pos : KcToolkit.posMap.values()) {
                double distance = Math.sqrt(Math.pow(pos.getPosX() - runningStatus.getPositionX(), 2) + Math.pow(pos.getPosY() - runningStatus.getPositionY(), 2));
                if (distance < minDistance) {
                    minDistance = distance;
                    nearestPos = pos;
                }
            }
            KcLocationDto locationDto = new KcLocationDto();
            locationDto.setIsReach(false);
            locationDto.setDistance(df.format(minDistance));
            if (runningStatus.getTaskStatus() == KcNavStatusEnum.NAV_FINISH.getStatus()) {
                locationDto.setCurrentCode(String.valueOf(runningStatus.getTargetPointId()));
                locationDto.setIsReach(true);
            } else if (runningStatus.getTaskStatus() == KcNavStatusEnum.NAV_GOING.getStatus()) {
                locationDto.setCurrentCode(Optional.ofNullable(nearestPos).map(RobotMapPos::getCode).orElse(""));
            } else {
                locationDto.setCurrentCode(Optional.ofNullable(nearestPos).map(RobotMapPos::getCode).orElse(""));
                locationDto.setIsReach(minDistance < 0.1d);
            }
            KcCacheUtil.put(KcCacheUtil.KEY_LOCATION_STATUS, locationDto);
        });
    }


    @Scheduled(fixedDelay = 1000L)
    public void checkStopStatus() {
        NetworkServiceThreadPool.dealEvent(() -> {
            //当前值
            KcStopStatusDto kcStopStatusDto = new KcStopStatusDto();
            int frontStopBarStatus = StrUtil.isNotBlank(KcCacheUtil.getVarCache("rep_emergency_f")) ? Integer.parseInt(KcCacheUtil.getVarCache("rep_emergency_f")) : 0;
            int backStopBarStatus = StrUtil.isNotBlank(KcCacheUtil.getVarCache("rep_emergency_b")) ? Integer.parseInt(KcCacheUtil.getVarCache("rep_emergency_b")) : 0;
            int hardStopStatus = StrUtil.isNotBlank(KcCacheUtil.getVarCache("rep_emergency_s")) ? Integer.parseInt(KcCacheUtil.getVarCache("rep_emergency_s")) : 0;
            kcStopStatusDto.setFrontStopBarStatus(frontStopBarStatus);
            kcStopStatusDto.setBackStopBarStatus(backStopBarStatus);
            kcStopStatusDto.setHardStopStatus(hardStopStatus);
            //上次值
            KcStopStatusDto lastKcStopStatusDto = Optional.ofNullable((KcStopStatusDto) KcCacheUtil.get(KcCacheUtil.KEY_STOP_STATUS)).orElse(new KcStopStatusDto());
            int lastFrontStopBarStatus = lastKcStopStatusDto.getFrontStopBarStatus();
            int lastBackStopBarStatus = lastKcStopStatusDto.getBackStopBarStatus();
            int lastHardStopStatus = lastKcStopStatusDto.getHardStopStatus();
            //充电状态和定位状态
            KcRobotRunningStatus runningStatus = (KcRobotRunningStatus) KcCacheUtil.get(KcCacheUtil.KEY_RUNNING_STATUS);
            int chargeStatus = runningStatus == null ? 0 : runningStatus.getIsCharging();
            KcLocationDto kcLocationDto = (KcLocationDto) KcCacheUtil.get(KcCacheUtil.KEY_LOCATION_STATUS);
            //语音播报
            //前防撞条压到充电桩的时候不播报
            if (frontStopBarStatus == 1 && !Objects.equal(kcLocationDto.getCurrentCode(), KcToolkit.chargePosCode) && publishFrontStopStatusVoiceLimiter.tryAcquire(10)) {
                PlayVoiceEvent playVoiceEvent = new PlayVoiceEvent();
                playVoiceEvent.setVoiceEnum(VoiceEnum.STOP_BAR);
                SpringUtil.getApplicationContext().publishEvent(playVoiceEvent);
            }
            //防撞条
            if (backStopBarStatus == 1 && chargeStatus != 1 && publishBackStopStatusVoiceLimiter.tryAcquire(10)) {
                PlayVoiceEvent playVoiceEvent = new PlayVoiceEvent();
                playVoiceEvent.setVoiceEnum(VoiceEnum.STOP_BAR);
                SpringUtil.getApplicationContext().publishEvent(playVoiceEvent);
            }
            //急停充电的时候不播报
            if (hardStopStatus == 1 && chargeStatus != 1 && publishHardStopStatusVoiceLimiter.tryAcquire(10)) {
                PlayVoiceEvent playVoiceEvent = new PlayVoiceEvent();
                playVoiceEvent.setVoiceEnum(VoiceEnum.HARD_STOP);
                SpringUtil.getApplicationContext().publishEvent(playVoiceEvent);
            }
            //状态改变了发布事件出去
            if (lastFrontStopBarStatus != kcStopStatusDto.getFrontStopBarStatus()
                    || lastBackStopBarStatus != kcStopStatusDto.getBackStopBarStatus()
                    || lastHardStopStatus != kcStopStatusDto.getHardStopStatus()) {
                SpringUtil.getApplicationContext().publishEvent(kcStopStatusDto);
                KcStopTriggerEvent kcStopTriggerEvent = BeanUtil.copyProperties(kcStopStatusDto, KcStopTriggerEvent.class);
                SpringUtil.getApplicationContext().publishEvent(kcStopTriggerEvent);
            }
            KcCacheUtil.put(KcCacheUtil.KEY_STOP_STATUS, kcStopStatusDto);

        });
    }


    @Scheduled(fixedDelay = 5000L)
    private void queryTasLanSenor() {
        NetworkServiceThreadPool.dealMessage(TasLan464Toolkit::query);
    }


    private static AtomicBoolean robotIsOnline = new AtomicBoolean(true);

    private static final RateLimiter changeOnlineStatusRateLimiter = RateLimiter.create(1);

    /**
     * 更新机器人上下线状态
     *
     * @throws InterruptedException
     */
    @Scheduled(fixedDelay = 1000)
    private void changeOnlineStatus() throws InterruptedException {
        try {
            KcRobotRunningStatus runningStatus = (KcRobotRunningStatus) KcCacheUtil.get(KcCacheUtil.KEY_RUNNING_STATUS);
            if (changeOnlineStatusRateLimiter.tryAcquire(300)) {//每5分钟纠正一次
                RobotOnlineEventDto dto = new RobotOnlineEventDto();
                dto.setOnlineStatus(runningStatus == null ? 0 : 1);
                SpringUtil.getApplicationContext().publishEvent(dto);
                robotIsOnline.set(runningStatus != null);
            } else {
                if (runningStatus == null) {//离线
                    if (robotIsOnline.compareAndSet(true, false)) {
                        //切离线
                        RobotOnlineEventDto dto = new RobotOnlineEventDto();
                        dto.setOnlineStatus(0);
                        SpringUtil.getApplicationContext().publishEvent(dto);
                    }
                } else {//在线
                    if (robotIsOnline.compareAndSet(false, true)) {
                        //切在线
                        RobotOnlineEventDto dto = new RobotOnlineEventDto();
                        dto.setOnlineStatus(1);
                        SpringUtil.getApplicationContext().publishEvent(dto);
                    }
                }
            }
        } catch (Exception e) {
            log.warn("更新机器人上下线状态异常", e);
        }
    }

}
